/*
 * OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
 *
 * Copyright (C) 2022 i2Nav Group, Wuhan University
 *
 *     Author : Hailiang Tang
 *    Contact : thl@whu.edu.cn
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef MARGINALIZATION_FACTOR_H
#define MARGINALIZATION_FACTOR_H

#include <ceres/ceres.h>
#include <memory>

#include "marginalization_info.h"

/*
MarginalizationFactor在优化问题中用于处理边缘化的因子。边缘化是一种减少优化问题维数的方法，常用于将某些变量从优化中去掉，
同时保留其对剩余变量的影响。通常用于图优化和状态估计（例如SLAM、BA等）中，
边缘化可以提高计算效率并减少内存使用。
*/
class MarginalizationFactor : public ceres::CostFunction {
public:
    MarginalizationFactor() = delete;//是禁止使用默认构造函数 MarginalizationFactor。
    
    //  构造函数负责初始化边缘化因子，并设定优化过程中所需的参数块和残差维度。

    /*  用于声明一个智能指针，具体来说是一个 shared_ptr，指向 MarginalizationInfo 类型的对象。
        std::shared_ptr<MarginalizationInfo> marg_info
    */
    explicit MarginalizationFactor(std::shared_ptr<MarginalizationInfo> marg_info)
        : marg_info_(std::move(marg_info)) {

        // 给定每个参数块数据大小
        /*
            remainedBlockSize() 是 MarginalizationInfo 类中的一个方法，
            返回一个包含每个参数块大小的容器（例如，std::vector<int>）
            
            mutable_parameter_block_sizes() 可能是一个成员方法，
            返回一个指向当前对象中存储参数块大小的容器的指针。

            The signature of the CostFunction (number and sizes of input parameter blocks and 
            number of outputs) is stored in CostFunction::parameter_block_sizes_ and 
            CostFunction::num_residuals_ respectively. User code inheriting from this 
            class is expected to set these two members with the corresponding accessors. 
            This information will be verified by the Problem when added 
            with Problem::AddResidualBlock().
            
            CostFunction（输入参数块的数量和大小以及输出的数量）分别存储在
             CostFunction::parameter_block_sizes_和 中CostFunction::num_residuals_。
             从此类继承的用户代码应使用相应的访问器设置这两个成员。
             此信息将在 Problem添加到时由进行验证Problem::AddResidualBlock()。
        */
        for (auto size : marg_info_->remainedBlockSize()) {
            mutable_parameter_block_sizes()->push_back(size);
        }

        // 残差大小
        set_num_residuals(marg_info_->remainedSize());
    }

    bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override {
        int marginalizaed_size = marg_info_->marginalizedSize();
        int remained_size      = marg_info_->remainedSize();

        const vector<int> &remained_block_index     = marg_info_->remainedBlockIndex();
        const vector<int> &remained_block_size      = marg_info_->remainedBlockSize();
        const vector<double *> &remained_block_data = marg_info_->remainedBlockData();

        Eigen::VectorXd dx(remained_size);
        for (size_t i = 0; i < remained_block_size.size(); i++) {
            int size  = remained_block_size[i];
            int index = remained_block_index[i] - marginalizaed_size;

            Eigen::VectorXd x  = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
            Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(remained_block_data[i], size);

            // dx = x - x0
            if (size == POSE_GLOBAL_SIZE) {
                Eigen::Quaterniond dq(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() *
                                      Eigen::Quaterniond(x(6), x(3), x(4), x(5)));

                dx.segment(index, 3)     = x.head<3>() - x0.head<3>();
                dx.segment(index + 3, 3) = 2.0 * dq.vec();
                if (dq.w() < 0) {
                    dx.segment<3>(index + 3) = -2.0 * dq.vec();
                }
            } else {
                dx.segment(index, size) = x - x0;
            }
        }

        // e = e0 + J0 * dx
        Eigen::Map<Eigen::VectorXd>(residuals, remained_size) =
            marg_info_->linearizedResiduals() + marg_info_->linearizedJacobians() * dx;

        if (jacobians) {

            for (size_t i = 0; i < remained_block_size.size(); i++) {
                if (jacobians[i]) {
                    int size       = remained_block_size[i];
                    int index      = remained_block_index[i] - marginalizaed_size;
                    int local_size = marg_info_->localSize(size);

                    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(
                        jacobians[i], remained_size, size);

                    // J = J0
                    jacobian.setZero();
                    jacobian.leftCols(local_size) = marg_info_->linearizedJacobians().middleCols(index, local_size);
                }
            }
        }

        return true;
    }

private:
    std::shared_ptr<MarginalizationInfo> marg_info_;
};

#endif // MARGINALIZATION_FACTOR_H
